/* --------------------------------------------------------------------------------
 * Example test for QtOpenGLViewer.
 *
 * Author: wlt027
 * Email: 544496077@qq.com
 * -------------------------------------------------------------------------------- */

#include "view_window.h"

#include <QApplication>
#include <QMouseEvent>
#include <iostream>
#include<cmath>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "liblas/liblas.hpp"

// add some spheres to our viewer as child objects on construction.
ViewWindow::ViewWindow(QWidget *parent) : QMainWindow(parent)
{


    //MDI Area
    {
        m_mdiArea = new QMdiArea(this);
        setCentralWidget(m_mdiArea);
        m_mdiArea->installEventFilter(this);
    }
    new3DView(true);
   entity_pc = new ccPointCloud("vertices");

}

ViewWindow::~ViewWindow()
{
    // printf("shut down\n");
}



ccGLWindow *ViewWindow::getActiveGLWindow()
{
    if (!m_mdiArea)
    {
        return 0;
    }

    QMdiSubWindow *activeSubWindow = m_mdiArea->activeSubWindow();
    if (activeSubWindow)
    {
        return GLWindowFromWidget(activeSubWindow->widget());
    }
    else
    {
        QList<QMdiSubWindow *> subWindowList = m_mdiArea->subWindowList();
        if (!subWindowList.isEmpty())
        {
            return GLWindowFromWidget(subWindowList[0]->widget());
        }
    }

    return 0;
}


ccGLWindow *ViewWindow::new3DView(bool allowEntitySelection)
{
    // assert(m_ccRoot && m_mdiArea);

    QWidget *viewWidget = nullptr;
    ccGLWindow *view3D = nullptr;
    
    CreateGLWindow(view3D, viewWidget, false);

    if (!viewWidget || !view3D)
    {
        ccLog::Error("Failed to create the 3D view");
        assert(false);
        return nullptr;
    }      

    view3D->setAutoPickPivotAtCenter(true);

    viewWidget->setMinimumSize(400, 300);

    m_mdiArea->addSubWindow(viewWidget);

    viewWidget->setAttribute(Qt::WA_DeleteOnClose);
    //viewWidget->setWindowFlags(Qt::CustomizeWindowHint | Qt::Tool);
    // view3D->setWindowFlags(Qt::FramelessWindowHint| Qt::Tool);
    
    // QMainWindow::statusBar()->showMessage(QString("New 3D View"), 2000);

    viewWidget->showMaximized();
    viewWidget->update();
    std::cout<<"created!"<<std::endl;
    return view3D;
}

void ViewWindow::addToDB(ccHObject *entity, ccGLWindow *detWin)
{
    assert(entity && detWin);

    entity->setDisplay_recursive(detWin);

    ccHObject *currentRoot = detWin->getSceneDB();
    if (currentRoot)
    {
        //already a pure 'root'
        if (currentRoot->isA(CC_TYPES::HIERARCHY_OBJECT))
        {
            currentRoot->addChild(entity);
        }
        else
        {
            ccHObject *root = new ccHObject("root");
            root->addChild(currentRoot);
            root->addChild(entity);
            detWin->setSceneDB(root);
        }
    }
    else
    {
        detWin->setSceneDB(entity);
    }

}

void ViewWindow::setPath(QString path){

    // readPCD(path);
    readLAS(path);
    readYaml(path);

}

void ViewWindow::setPoints(QVector<QVector<QPointF>> vec){
    removePlane();
    for(auto &p: vec){
        std::cout<<"draw plane"<<std::endl;
        drawPlane(p[0].x(), p[0].y(), p[1].x(), p[1].y());
    }
}


void ViewWindow::drawPlane(int x1, int y1, int x2, int y2){


    float map_x1 = x1 * currentImageRes + originMapX;
    float map_y1 = (currentImageHeight - y1 - 1) * currentImageRes + originMapY;

    float map_x2 = x2 * currentImageRes + originMapX;
    float map_y2 = (currentImageHeight - y2 - 1) * currentImageRes + originMapY;

    ccGLWindow *detWin = getActiveGLWindow();
    ccPointCloud* pc = new ccPointCloud("pc");
    CCVector3 P1(map_x1, map_y1, 0);
    CCVector3 P2(map_x2, map_y2, 0);
    CCVector3 P3((map_x1 + map_x2)/ 2, (map_y1 + map_y2)/ 2, 5);
    pc->addPoint(P1);
    pc->addPoint(P2);
    pc->addPoint(P3);
    CCLib::GenericIndexedCloudPersist* g_cloud = 0;
    g_cloud = static_cast<CCLib::GenericIndexedCloudPersist*>(pc);
    ccPlane* pPlane = ccPlane::Fit(g_cloud);
    ccColor::Rgb rgb_p = ccColor::Rgb(244, 164, 95);
    pPlane->setColor(rgb_p);
    pPlane->showColors(true);
    box.push_back(pPlane);
    addToDB(pPlane, detWin);

    float k1 = (map_x1 - map_x2) / (map_y2 - map_y1);
   
    float x1_up = map_x1 + 0.05*cos(atan(k1));
    float y1_up = map_y1 + 0.05*sin(atan(k1));
    float x1_down = map_x1 - 0.05*cos(atan(k1));
    float y1_down = map_y1 - 0.05*sin(atan(k1));

    float x2_up = map_x2 + 0.05*cos(atan(k1));
    float y2_up = map_y2 + 0.05*sin(atan(k1));
    float x2_down = map_x2 - 0.05*cos(atan(k1));
    float y2_down = map_y2 - 0.05*sin(atan(k1));
    std::cout<<"x y: "<<x1_up<<" "<<y1_up<<" "<<x1_down<<" "<<y1_down<<" "<<
                x2_up<<" "<<y2_up<<" "<<x2_down<<" "<<y2_down<<std::endl;
    // front
    CCVector3 P01(x1_up, y1_up, 0);
    CCVector3 P02(x2_up, y2_up, 0);
    CCVector3 P03((x1_up + x2_up)/ 2, (y1_up + y2_up)/ 2, 5);
    drawBox(P01, P02, P03);
    // back
    CCVector3 P4(x1_down, y1_down, 0);
    CCVector3 P5(x2_down, y2_down, 0);
    CCVector3 P6((x1_down + x2_down)/ 2, (y1_down + y2_down)/ 2, 5);
    drawBox(P4, P5, P6);
    // bottom
    CCVector3 P7(x1_down, y1_down, 0);
    CCVector3 P8(x1_up, y1_up, 0);
    CCVector3 P9(x2_down, y2_down, 0);
    drawBox(P7, P8, P9);
    // top
    CCVector3 P10(x1_down, y1_down, 5);
    CCVector3 P11(x1_up, y1_up, 5);
    CCVector3 P12(x2_down, y2_down, 5);
    drawBox(P10, P11, P12);
    
    detWin->redraw();

}

void ViewWindow::drawBox(CCVector3 P1, CCVector3 P2, CCVector3 P3)
{
    ccGLWindow *detWin = getActiveGLWindow();

    ccPointCloud* pc_1 = new ccPointCloud("pc_1");
    pc_1->addPoint(P1);
    pc_1->addPoint(P2);
    pc_1->addPoint(P3);
    CCLib::GenericIndexedCloudPersist* g_cloud_1 = 0;
    g_cloud_1 = static_cast<CCLib::GenericIndexedCloudPersist*>(pc_1);
    ccPlane* pPlane = ccPlane::Fit(g_cloud_1);
    ccColor::Rgb rgb_p = ccColor::Rgb(255, 127, 80);
    pPlane->setColor(rgb_p);
    pPlane->showColors(true);

    box.push_back(pPlane);
    addToDB(pPlane, detWin);
    detWin->redraw();

}

void ViewWindow::removePlane()
{
    
    ccGLWindow *detWin = getActiveGLWindow();
    detWin->setInteractionMode(ccGLWindow::TRANSFORM_CAMERA());

    // ccHObject *currentRoot = detWin->getSceneDB();
    // std::cout<<"ChildrenNumber "<<currentRoot->getChildrenNumber()<<std::endl;

    if(box.empty()) return;
    for(auto &plane: box){
        if (plane->getDisplay())
		{
			static_cast<ccGLWindow*>(plane->getDisplay())->removeFromOwnDB(plane);
		}        
		delete plane;        
		plane= nullptr;        
        box.pop_back();

    }
    box.clear();
    detWin->redraw();

}

void ViewWindow::getColorRamp(float minz, float maxz, float pz, float &r, float &g, float &b)
{

    r = 0;
    g = 0;
    b = 1;
    float index = (pz - minz) / (maxz - minz) * 8.;
    if (index <= 2.)
    {
        r = 0;
        g = index * 0.5;
        b = 1;
    }
    else if (index > 2. && index <= 4.)
    {
        r = 0;
        g = 1;
        b = 1 - (index - 2) * 0.5;
    }
    else if (index > 4. && index <= 6.)
    {
        r = (index - 4) * 0.5;
        g = 1;
        b = 0;
    }
    else if (index > 6. && index <= 8.)
    {
        r = 1;
        g = 1 - (index - 6) * 0.5;
        b = 0;
    }
    else
    {
        r = 1;
        g = 0;
        b = 0;
    }
}

void ViewWindow::readPCD(QString path){
    std::string pcd_file = path.toStdString() + "/map.pcd";
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_file, *cloud);
    ccGLWindow *detWin = getActiveGLWindow();
    float r, g, b;
    ccColor::Rgb rgb;
    uint16_t num = cloud->size();
     entity_pc->reserve(num);
     for (uint16_t i = 0; i < num; i++)
     {
        CCVector3 P11((cloud->points)[i].x, (cloud->points)[i].y, (cloud->points)[i].z);
        entity_pc->addPoint(P11);
        float z = (cloud->points)[i].z;
        getColorRamp(-1, 5, z, r, g, b);
        rgb = ccColor::Rgb(r*255, g*255, b*255);
        entity_pc->resizeTheRGBTable(true);
        entity_pc->setPointColor(i, rgb);
        entity_pc->showColors(true);
     }

     addToDB(entity_pc, detWin);

}

void ViewWindow::readLAS(QString path){
    using namespace std;
    std::string las_file = path.toStdString() + "/map.las";
    cout<<"las file "<<las_file<<endl;
    std::ifstream ifs(las_file, std::ios::in | std::ios::binary);
    liblas::ReaderFactory f;
    liblas::Reader reader = f.CreateWithStream(ifs);

    //读取LAS文件信息头
    liblas::Header const& header = reader.GetHeader();
    double maxX = header.GetMaxX();
    double minX = header.GetMinX();
    double maxY = header.GetMaxY();
    double minY = header.GetMinY();
    double maxZ = header.GetMaxZ();
    double minZ = header.GetMinZ();
    int nbPoints = header.GetPointRecordsCount();
    string signature = header.GetFileSignature();

    cout << "maxX: " << maxX << endl;
    cout << "minX: " << minX << endl;
    cout << "maxY: " << maxY << endl;
    cout << "minY: " << minY << endl;
    cout << "maxZ: " << maxZ << endl;
    cout << "minZ: " << minZ << endl;
    cout << "点个数: " << nbPoints << endl;
    cout << "signature: " << signature << endl;

    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    cloud.width = nbPoints;
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(cloud.width * cloud.height);

    int i = 0;
    uint16_t r1, g1, b1;
    int r2, g2, b2;
    uint32_t rgb;

    ccGLWindow *detWin = getActiveGLWindow();
    float r, g, b;
    ccColor::Rgb rgb_las;

    //读取点云坐标和色彩信息
    while (reader.ReadNextPoint())
    {
        // 获取las数据的x，y，z信息
        cloud.points[i].x = reader.GetPoint().GetX();
        cloud.points[i].y = reader.GetPoint().GetY();
        cloud.points[i].z = reader.GetPoint().GetZ();

        // 获取las数据的r，g，b信息
        r1 = reader.GetPoint().GetColor().GetRed();
        g1 = reader.GetPoint().GetColor().GetGreen();
        b1 = reader.GetPoint().GetColor().GetBlue();
        r2 = ceil(((float)r1 / 65536)*(float)256);
        g2 = ceil(((float)g1 / 65536)*(float)256);
        b2 = ceil(((float)b1 / 65536)*(float)256);

        rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
        cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);
        
        
        getColorRamp(-1, 5, cloud.points[i].z, r, g, b);
        rgb_las = ccColor::Rgb(r*255, g*255, b*255);


        CCVector3 P11(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z);
        entity_pc->addPoint(P11);

        // rgb_las = ccColor::Rgb(r2, g2, b2);
        entity_pc->resizeTheRGBTable(true);
        entity_pc->setPointColor(i, rgb_las);
        entity_pc->showColors(true);

        i++;
    }
    addToDB(entity_pc, detWin);
}

void ViewWindow::setCurrentHeight(int height){
    currentImageHeight = height;

}


void ViewWindow::readYaml(QString path){
    std::string yaml_file =  path.toStdString() + "/map.yaml";
    alive::ParameterReader::Ptr param_reader = alive::ParameterReader::create(yaml_file);

    if (param_reader)
    {
        std::string strRes = param_reader->getData("resolution");
        currentImageRes = atof(strRes.c_str());

        std::string strOri = param_reader->getData("origin");
        std::string strTmpX = strOri.substr(strOri.find_first_of("[") + 1, strOri.find_first_of(".") + 1);
        originMapX = atof(strTmpX.c_str());

        std::string strTmpY = strOri.substr(strOri.find_first_of(",") + 1, strOri.find_last_of(","));
        strTmpY = strTmpY.substr(0, strTmpY.find_first_of(","));

        originMapY = atof(strTmpY.c_str());
    }
}
